import numpy as np
import json


def print_diff_rst(rear_leg_trajectory):
    array = np.array(rear_leg_trajectory, dtype=np.float32)
    # 取出array的偶数列
    array_even = array[0::2]
    # 取出array的奇数列
    array_odd = array[1::2]
    # 将奇数列的值减去偶数列的值
    array_diff: np.ndarray = array_odd - array_even
    # print(array_odd)
    # print("-----------------")
    # print(array_even)
    array_diff *= 18
    # print(array_diff)
    json_str = json.dumps([array_diff.tolist()])
    print(json_str)

    x_lst = array_diff[:, 0]
    y_lst = array_diff[:, 1]
    print(f"way_point_x = {x_lst.tolist()}")
    print(f"way_point_y = {y_lst.tolist()}")




def rear_leg():
    rear_leg_trajectory = [
        [12.08, 6.54], [14.10, 10.06],
        [12.08, 6.61], [14.24, 10.04],
        [12.08, 6.65], [13.68, 10.08],
        [12.08, 6.70], [12.75, 9.94],
        [12.08, 6.65], [12.21, 10.02],
        [12.08, 6.62], [11.59, 10.17],
        [12.08, 6.59], [11.29, 10.32],
        [12.08, 6.64], [11.61, 10.32],
        [12.08, 6.71], [11.92, 10.32],
        [12.08, 6.75], [12.14, 10.32],
        [12.08, 6.69], [12.56, 10.32],
        [12.08, 6.60], [12.90, 10.32],
        [12.08, 6.58], [13.22, 10.32],
        [12.08, 6.54], [14.10, 10.06],
    ]
    print_diff_rst(rear_leg_trajectory)


def front_leg():
    front_leg_trajectory = [
        [9.16, 6.37], [7.60, 10.06],
        [9.16, 6.43], [7.99, 10.32],
        [9.16, 6.54], [8.30, 10.32],
        [9.16, 6.59], [8.63, 10.32],
        [9.16, 6.54], [9.27, 10.32],
        [9.16, 6.51], [9.69, 10.32],
        [9.16, 6.44], [10.23, 10.32],
        [9.16, 6.50], [10.86, 10.03],
        [9.16, 6.57], [10.02, 9.97],
        [9.16, 6.61], [9.45, 9.88],
        [9.16, 6.55], [8.50, 9.98],
        [9.16, 6.45], [7.82, 10.02],
        [9.16, 6.39], [7.44, 10.02],
        [9.16, 6.37], [7.60, 10.06],
    ]
    print_diff_rst(front_leg_trajectory)



def trot_trajectory():
    front_leg()
    # rear_leg()


if __name__ == '__main__':
    trot_trajectory()
